#include "balance.h"
#include "pid.h"
#include "mpu6050.h"
#include "motor.h"
#include "inv_mpu_dmp_motion_driver.h"
#include "kalman.h"

extern motor_t motor;
//extern pid_t  pid_balance;
//extern pid_t pid_spd;
extern float Pitch,Roll,Yaw;
extern int16_t Encoder_Left,Encoder_Right;
float balance_pid[9] = {165,0,0.28
													,75,0.37,0
													,0,0,0};//165,0,0.28,75,0.37,0,50//直立环，速度环，转向环180,0.3,30,0.15
balance_t balance;
extern short gyro[3];
int spd_out,ver_out,turn_out;		
int16_t val;		//给定速度											
//extern Kalman_t KalmanX;
short gyro_yy;
void balance_calc(void)
{
	balance.Med = -1;
	gyro_yy = gyro[1];
	//角度环
	balance.upr_get = Pitch;
	balance.upr_ref = balance.Med;
	ver_out = Vertical(balance.upr_ref,balance.upr_get,balance.gro_spd);
	
	//速度环
	balance.spd_get = -(Encoder_Left + Encoder_Right);
	balance.spd_ref = 0;
	spd_out = Velocity(balance.spd_ref,balance.spd_get );
	
	//转向环
	turn_out = turn(Encoder_Left, Encoder_Right);
	
	motor.PWM_out = -ver_out + spd_out ;
	motor.motor1_out = motor.PWM_out + turn_out;
	motor.motor2_out = motor.PWM_out - turn_out;
	
	Limit(&motor.motor1_out,&motor.motor2_out);
	Load(motor.motor1_out,motor.motor2_out);
}

